#include "test_ref_smoother.h"

void TestRefSmoother::processAndPublishData()
{

  std::vector<double> map_1_x, map_1_y;
  std::ifstream file(csv_file_path);
  std::string line;

  while (std::getline(file, line)) {
    std::stringstream linestream(line);
    std::string cell;
    std::vector<std::string> parsedRow;
    while (std::getline(linestream, cell, ',')) {
      parsedRow.push_back(cell);
    }
    // 只处理前两列数据
    if (parsedRow.size() >= 2) {
      map_1_x.push_back(std::stod(parsedRow[0]));
      map_1_y.push_back(std::stod(parsedRow[1]));
    }
  }
  // 初始化 hdmap_way_points
  hdmap_way_points = Eigen::MatrixXd::Zero(map_1_x.size(), 3);
  for (size_t i = 0; i < map_1_x.size(); ++i) {
    hdmap_way_points(i, 0) = map_1_x[i];
    hdmap_way_points(i, 1) = map_1_y[i];
    hdmap_way_points(i, 2) = 0; // 填充0作为第三列数据
  }

  Gl.global_refer(hdmap_way_points);
  hasProcessed = true;
}

/* void TestRefSmoother::initialpose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{

  if(Start_local_planning == "0")
  {
    initialpose_set = true;
    init_pose_.position.x = msg->pose.pose.position.x;
    init_pose_.position.y = msg->pose.pose.position.y;
    init_pose_.orientation = msg->pose.pose.orientation;
    /* std::cout << "start_x:" << msg->pose.pose.position.x << std::endl;
    std::cout << "start_y:" << msg->pose.pose.position.y << std::endl;
    std::cout << "theta_start:" << tf::getYaw(msg->pose.pose.orientation) << std::endl; 
  }

} */

void TestRefSmoother::move_base_simple_goal(const geometry_msgs::PoseStamped::ConstPtr &msg)
{

  if(Start_local_planning == "0")
  {
    move_base_simple_goal_set = true;
    goal_pose_.position.x = msg->pose.position.x;
    goal_pose_.position.y = msg->pose.position.y;
    goal_pose_.orientation = msg->pose.orientation;
  }

}

void TestRefSmoother::Odom(const nav_msgs::Odometry &msg)
{
  car_odom_ = msg;
  global_odom_x  =  car_odom_.pose.pose.position.x ;
  global_odom_y  =  car_odom_.pose.pose.position.y ;
  global_odom_yaw  =  tf::getYaw(car_odom_.pose.pose.orientation) ;
}

void TestRefSmoother::AmclPose(const geometry_msgs::PoseWithCovarianceStamped &msg){
  car_locat_pose_= msg;
}

void TestRefSmoother::Theta(const geometry_msgs::PoseArray &theta)
{
  thetas.clear();
  for (const auto &pose : theta.poses) {
      thetas.push_back(pose.position.x);
  }
}

void TestRefSmoother::Kappa(const geometry_msgs::PoseArray &kappa)
{
  kappas.clear();
  for (const auto &pose : kappa.poses) {
      kappas.push_back(pose.position.x);
  }
}

void TestRefSmoother::Traj_speed(const waypoint_msgs::WaypointArray::ConstPtr &msg)
{
  dynamic_points.poses.clear();
  dynamic_points.header.frame_id = Frame_id;
  dynamic_points.header.stamp = ros::Time::now();
  speeds.clear();
  for (const auto &waypoint : msg->waypoints) {
      pose_stamp.header.frame_id = Frame_id;
      pose_stamp.header.stamp = ros::Time::now();
      pose_stamp.pose.position.x = waypoint.pose.pose.position.x;
      pose_stamp.pose.position.y = waypoint.pose.pose.position.y;
      pose_stamp.pose.position.z = 0;
      dynamic_points.poses.push_back(pose_stamp);
      speeds.push_back(waypoint.twist.twist.linear.x); // 速度
  }
}

void TestRefSmoother::Acceleration(const geometry_msgs::PoseArray &acceleration)
{
  accelerations.clear();
  for (const auto &pose : acceleration.poses) {
      accelerations.push_back(pose.position.x);
  }
}

bool TestRefSmoother::Vehical_Odom_Stop(const geometry_msgs::Pose &goal_point, nav_msgs::Odometry &car_locat)
{
  // 获取车的里程位置
  double x = car_locat.pose.pose.position.x;
  double y = car_locat.pose.pose.position.y;

  // 获取路径点的最后一个位置
  double xt = goal_point.position.x;
  double yt = goal_point.position.y;
  // 判断如果两点坐标接近

  double dis = sqrt((x - xt) * (x - xt) + (y - yt) * (y - yt));
  if (dis < goal_distanse)
  {
    return true;
  }
  return false;
}

bool TestRefSmoother::Vehical_Locat_Stop(const geometry_msgs::Pose &goal_point, geometry_msgs::PoseWithCovarianceStamped &locat)
{
  // 获取车的里程位置
  double x = locat.pose.pose.position.x;
  double y = locat.pose.pose.position.y;

  // 获取路径点的最后一个位置
  double xt = goal_point.position.x;
  double yt = goal_point.position.y;
  // 判断如果两点坐标接近

  double dis = sqrt((x - xt) * (x - xt) + (y - yt) * (y - yt));
  if (dis < goal_distanse)
  {
    return true;
  }
  return false;
}

void TestRefSmoother::LocalStart(const std_msgs::String::ConstPtr &go)
{
  Start_local_planning = go->data.c_str();
}

void TestRefSmoother::startThread() {
  ros::Rate loop_rate(10);
  while (nh.ok()) {
    if (move_base_simple_goal_set) {
      processAndPublishData();
      if (hasProcessed) {
        break;
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

void TestRefSmoother::public_control_thread(void)
{
  ros::Rate loop_rate(10);
  turn_angle = 0;
  out_index_ = 0;
  Eigen::MatrixXd Q;
  Eigen::MatrixXd R;
  Q.setZero(4, 4);
  R.setZero(2, 2);
  while (nh.ok())
  {
    if (move_base_simple_goal_set == true && Start_local_planning == "1")
    {
      // is_vehical_stop_set = Vehical_Odom_Stop(goal_pose_, car_odom_); // 判断是否到达终点
      is_vehical_stop_set = Vehical_Locat_Stop(goal_pose_, car_locat_pose_); // 判断是否到达终点
      // Lqr.lqr_control(car_odom_, dynamic_points, kappas, thetas, speeds, turn_angle, out_index_);
      Mpc.mpc_control(car_odom_, dynamic_points, kappas, thetas, speeds, turn_angle, out_index_);
      // Lqr.lqr_locat_control(car_locat_pose_, dynamic_points, kappas, thetas, speeds, turn_angle, out_index_);
      msg_ros.header.stamp = ros::Time::now();
      msg_ros.header.frame_id = "map";
      msg_ros.vector.y = turn_angle;          // 转角
      if (is_vehical_stop_set == false) // 未到达终点
      {
        if (out_index_ < speeds.size()) {
          // car_speed = speeds[out_index_];
          car_speed = out_index_;
          msg_ros.vector.x = car_speed; // ros仿真直接赋值速度
          control_data_pub_.publish(msg_ros);
          // ROS_INFO("speed: %2f, turn_angle: %2f", car_speed, turn_angle);
        } else {
          car_speed = 0.0;
          // car_speed = out_index_;
          msg_ros.vector.x = 0.0; // ros仿真直接赋值速度
          control_data_pub_.publish(msg_ros);
          // 处理错误情况
          ROS_ERROR("Index out of bounds: %f", out_index_);
        }
      }else{
        ROS_WARN("Arrive at goal");
        msg_ros.vector.x = 0;
        control_data_pub_.publish(msg_ros);
        move_base_simple_goal_set = false;
        hasProcessed = false;
        Start_local_planning = "0";
        car_speed = 0;
      }
    }else if (move_base_simple_goal_set == true && Start_local_planning == "2"){
      msg_ros.vector.x = 0;
      control_data_pub_.publish(msg_ros);
      car_speed = 0;
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

TestRefSmoother::TestRefSmoother(){

  ros::param::get("csv_file_path", csv_file_path);
  std::cout << YELLOW << "csv_file_path: " << csv_file_path << RESET << std::endl;
  if (csv_file_path.empty()) {
      ROS_ERROR("Failed to get parameter 'csv_file_path' parameter. Ensure it is set correctly in the launch file.");
      return;
  }
  ros::Rate loop_rate(10);
  //初始化参数标志
  Start_local_planning = "0";
  // initialpose_set = false;
  move_base_simple_goal_set = false;
  hasProcessed = false;
  is_vehical_stop_set = true;
  goal_distanse = 0.5;
  Mpc.Init();
  car_speed=0;

  // 读取csv文件的路经点，进行平滑处理
  // hdmap_way_point_ = nh.advertise<nav_msgs::Path>("hdmap_way_points_path", 10);
  
  control_data_pub_ = nh.advertise<geometry_msgs::Vector3Stamped>("/xyl/car_control", 10);                      // ros仿真时发布车的控制数据

  // initialpose_ = nh.subscribe("/initialpose", 10, &TestRefSmoother::initialpose, this);
  move_base_simple_goal_ = nh.subscribe("/move_base_simple/goal", 10, &TestRefSmoother::move_base_simple_goal, this);

  odom_ = nh.subscribe("/odom", 10, &TestRefSmoother::Odom, this);
  AmclPose_ = nh.subscribe("/laserpose", 10, &TestRefSmoother::AmclPose, this);
  theta_ = nh.subscribe("/xyl/planning/dynamic_paths_t", 10, &TestRefSmoother::Theta, this);
  kappa_ = nh.subscribe("/xyl/planning/dynamic_paths_k", 10, &TestRefSmoother::Kappa, this);
  traj_speed_ = nh.subscribe("/xyl/planning/dynamic_waypoints", 10, &TestRefSmoother::Traj_speed, this);
  acceleration_ = nh.subscribe("/xyl/planning/dynamic_paths_a", 10, &TestRefSmoother::Acceleration, this); // 发布局部轨迹的加速度
  Start_local_ = nh.subscribe("/xyl/planning/Start_Dynamic", 10, &TestRefSmoother::LocalStart, this);

  task_thread_ = std::thread(&TestRefSmoother::startThread, this);

  public_control_thread_ = new boost::thread(boost::bind(&TestRefSmoother::public_control_thread, this));

  
}

TestRefSmoother::~TestRefSmoother(){
  task_thread_.detach();
  delete public_control_thread_;
}
